#if EMPL

#include "inv_mpl_lib_easy_use.h"

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>

#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "invensense.h"
#include "invensense_adv.h"
#include "eMPL_outputs.h"
#include "mltypes.h"
#include "mpu.h"

/* Data read from MPL. */
#define PRINT_ACCEL     (0x01)
#define PRINT_GYRO      (0x02)
#define PRINT_QUAT      (0x04)
#define PRINT_COMPASS   (0x08)
#define PRINT_EULER     (0x10)
#define PRINT_ROT_MAT   (0x20)
#define PRINT_HEADING   (0x40)
#define PRINT_PEDO      (0x80)
#define PRINT_LINEAR_ACCEL (0x100)
#define PRINT_GRAVITY_VECTOR (0x200)

#define ACCEL_ON        (0x01)
#define GYRO_ON         (0x02)
#define COMPASS_ON      (0x04)

#define MOTION          (0)
#define NO_MOTION       (1)

/* Starting sampling rate. */
#define DEFAULT_MPU_HZ  (20)

#define FLASH_SIZE      (512)
#define FLASH_MEM_START ((void*)0x1800)

#define PEDO_READ_MS    (1000)
#define TEMP_READ_MS    (500)
#define COMPASS_READ_MS (100)
struct rx_s {
    unsigned char header[3];
    unsigned char cmd;
};
struct hal_s {
    unsigned char lp_accel_mode;
    unsigned char sensors;
    unsigned char dmp_on;
    unsigned char wait_for_tap;
    volatile unsigned char new_gyro;
    unsigned char motion_int_mode;
    unsigned long no_dmp_hz;
    unsigned long next_pedo_ms;
    unsigned long next_temp_ms;
    unsigned long next_compass_ms;
    unsigned int report;
    unsigned short dmp_features;
    struct rx_s rx;
};
static struct hal_s hal = {0};

/* USB RX binary semaphore. Actually, it's just a flag. Not included in struct
 * because it's declared extern elsewhere.
 */
volatile unsigned char rx_new;

unsigned char *mpl_key = (unsigned char*)"eMPL 5.1";

/* Platform-specific information. Kinda like a boardfile. */
struct platform_data_s {
    signed char orientation[9];
};

/* The sensors can be mounted onto the board in any orientation. The mounting
 * matrix seen below tells the MPL how to rotate the raw data from the
 * driver(s).
 * TODO: The following matrices refer to the configuration on internal test
 * boards at Invensense. If needed, please modify the matrices to match the
 * chip-to-body matrix for your particular set up.
 */
static struct platform_data_s gyro_pdata = {
    .orientation = { 1, 0, 0,
                     0, 1, 0,
                     0, 0, 1}
};

#if defined MPU9150 || defined MPU9250
static struct platform_data_s compass_pdata = {
    .orientation = { 0, 1, 0,
                     1, 0, 0,
                     0, 0,-1}
};
#define COMPASS_ENABLED 1
#elif defined AK8975_SECONDARY
static struct platform_data_s compass_pdata = {
    .orientation = {-1, 0, 0,
                     0, 1, 0,
                     0, 0,-1}
};
#define COMPASS_ENABLED 1
#elif defined AK8963_SECONDARY
static struct platform_data_s compass_pdata = {
    .orientation = {-1, 0, 0,
                     0,-1, 0,
                     0, 0, 1}
};
#define COMPASS_ENABLED 1
#endif

/////////////////////////////////////////////////////////////////////////////
#include "stm32f0xx_hal.h"

#define q16  65536.0f

#define BUF_SIZE        (256)
#define PACKET_LENGTH   (23)

#define PACKET_DEBUG    (1)
#define PACKET_QUAT     (2)
#define PACKET_DATA     (3)

typedef enum {
    PACKET_DATA_ACCEL = 0,
    PACKET_DATA_GYRO,
    PACKET_DATA_COMPASS,
    PACKET_DATA_QUAT,
    PACKET_DATA_EULER,
    PACKET_DATA_ROT,
    PACKET_DATA_HEADING,
    PACKET_DATA_LINEAR_ACCEL,
    NUM_DATA_PACKETS
} eMPL_packet_e;

extern int get_tick_count(unsigned long *count);

void inv_mpl_delay_ms(unsigned int time)
{
	HAL_Delay(time);
}

int _MLPrintLog (int priority, const char* tag, const char* fmt, ...)
{
    va_list args;
    int length, ii;
    char buf[BUF_SIZE], out[PACKET_LENGTH], this_length;

    /* This can be modified to exit for unsupported priorities. */
    switch (priority) {
    case MPL_LOG_UNKNOWN:
    case MPL_LOG_DEFAULT:
    case MPL_LOG_VERBOSE:
    case MPL_LOG_DEBUG:
    case MPL_LOG_INFO:
    case MPL_LOG_WARN:
    case MPL_LOG_ERROR:
    case MPL_LOG_SILENT:
        break;
    default:
        return 0;
    }

    va_start(args, fmt);

    length = vsprintf(buf, fmt, args);
    if (length <= 0) {
        va_end(args);
        return length;
    }

    memset(out, 0, PACKET_LENGTH);
    out[0] = '$';
    out[1] = PACKET_DEBUG;
    out[2] = priority;
    out[21] = '\r';
    out[22] = '\n';
    for (ii = 0; ii < length; ii += (PACKET_LENGTH-5)) {
#define min(a,b) ((a < b) ? a : b)
        this_length = min(length-ii, PACKET_LENGTH-5);
        memset(out+3, 0, 18);
        memcpy(out+3, buf+ii, this_length);
        // cdcSendDataInBackground((BYTE*)out, PACKET_LENGTH, CDC0_INTFNUM, 0);
        printf("%s\r\n", out);
    }
    va_end(args);

    return 0;
}

void eMPL_send_quat(long *quat)
{
    char out[PACKET_LENGTH];
    if (!quat)
        return;
    memset(out, 0, PACKET_LENGTH);
    out[0] = '$';
    out[1] = PACKET_QUAT;
    out[3] = (char)(quat[0] >> 24);
    out[4] = (char)(quat[0] >> 16);
    out[5] = (char)(quat[0] >> 8);
    out[6] = (char)quat[0];
    out[7] = (char)(quat[1] >> 24);
    out[8] = (char)(quat[1] >> 16);
    out[9] = (char)(quat[1] >> 8);
    out[10] = (char)quat[1];
    out[11] = (char)(quat[2] >> 24);
    out[12] = (char)(quat[2] >> 16);
    out[13] = (char)(quat[2] >> 8);
    out[14] = (char)quat[2];
    out[15] = (char)(quat[3] >> 24);
    out[16] = (char)(quat[3] >> 16);
    out[17] = (char)(quat[3] >> 8);
    out[18] = (char)quat[3];
    out[21] = '\r';
    out[22] = '\n';
    printf("%s\r\n", out);
    // cdcSendDataInBackground((BYTE*)out, PACKET_LENGTH, CDC0_INTFNUM, 100);
}

void eMPL_send_data(unsigned char type, long *data)
{
    char out[PACKET_LENGTH];
    if (!data)
        return;
    memset(out, 0, PACKET_LENGTH);
    out[0] = '$';
    out[1] = PACKET_DATA;
    out[2] = type;
    out[21] = '\r';
    out[22] = '\n';
    switch (type) {
    /* Two bytes per-element. */
    case PACKET_DATA_ROT:
        out[3] = (char)(data[0] >> 24);
        out[4] = (char)(data[0] >> 16);
        out[5] = (char)(data[1] >> 24);
        out[6] = (char)(data[1] >> 16);
        out[7] = (char)(data[2] >> 24);
        out[8] = (char)(data[2] >> 16);
        out[9] = (char)(data[3] >> 24);
        out[10] = (char)(data[3] >> 16);
        out[11] = (char)(data[4] >> 24);
        out[12] = (char)(data[4] >> 16);
        out[13] = (char)(data[5] >> 24);
        out[14] = (char)(data[5] >> 16);
        out[15] = (char)(data[6] >> 24);
        out[16] = (char)(data[6] >> 16);
        out[17] = (char)(data[7] >> 24);
        out[18] = (char)(data[7] >> 16);
        out[19] = (char)(data[8] >> 24);
        out[20] = (char)(data[8] >> 16);
        break;
    /* Four bytes per-element. */
    /* Four elements. */
    case PACKET_DATA_QUAT:
        out[15] = (char)(data[3] >> 24);
        out[16] = (char)(data[3] >> 16);
        out[17] = (char)(data[3] >> 8);
        out[18] = (char)data[3];
    /* Three elements. */
    case PACKET_DATA_ACCEL:
    case PACKET_DATA_GYRO:
    case PACKET_DATA_COMPASS:
    case PACKET_DATA_EULER:
        out[3] = (char)(data[0] >> 24);
        out[4] = (char)(data[0] >> 16);
        out[5] = (char)(data[0] >> 8);
        out[6] = (char)data[0];
        out[7] = (char)(data[1] >> 24);
        out[8] = (char)(data[1] >> 16);
        out[9] = (char)(data[1] >> 8);
        out[10] = (char)data[1];
        out[11] = (char)(data[2] >> 24);
        out[12] = (char)(data[2] >> 16);
        out[13] = (char)(data[2] >> 8);
        out[14] = (char)data[2];
        break;
    case PACKET_DATA_HEADING:
        out[3] = (char)(data[0] >> 24);
        out[4] = (char)(data[0] >> 16);
        out[5] = (char)(data[0] >> 8);
        out[6] = (char)data[0];
        break;
    default:
        return;
    }
    printf("%s\r\n", out);
    // cdcSendDataInBackground((BYTE*)out, PACKET_LENGTH, CDC0_INTFNUM, 100);
}
/////////////////////////////////////////////////////////////////////////////
/* Get data from MPL.
 * TODO: Add return values to the inv_get_sensor_type_xxx APIs to differentiate
 * between new and stale data.
 */
static void read_from_mpl(void)
{
    long msg, data[9];
    int8_t accuracy;
    unsigned long timestamp;
    float float_data[3] = {0};

    if (inv_get_sensor_type_quat(data, &accuracy, (inv_time_t*)&timestamp)) {
       /* Sends a quaternion packet to the PC. Since this is used by the Python
        * test app to visually represent a 3D quaternion, it's sent each time
        * the MPL has new data.
        */
        eMPL_send_quat(data);

        /* Specific data packets can be sent or suppressed using USB commands. */
        if (hal.report & PRINT_QUAT)
            eMPL_send_data(PACKET_DATA_QUAT, data);
    }

    if (hal.report & PRINT_ACCEL) {
        if (inv_get_sensor_type_accel(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_ACCEL, data);
    }
    if (hal.report & PRINT_GYRO) {
        if (inv_get_sensor_type_gyro(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_GYRO, data);
    }
#ifdef COMPASS_ENABLED
    if (hal.report & PRINT_COMPASS) {
        if (inv_get_sensor_type_compass(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_COMPASS, data);
    }
#endif
    if (hal.report & PRINT_EULER) {
        if (inv_get_sensor_type_euler(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_EULER, data);
    }
    if (hal.report & PRINT_ROT_MAT) {
        if (inv_get_sensor_type_rot_mat(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_ROT, data);
    }
    if (hal.report & PRINT_HEADING) {
        if (inv_get_sensor_type_heading(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_HEADING, data);
    }
    if (hal.report & PRINT_LINEAR_ACCEL) {
        if (inv_get_sensor_type_linear_acceleration(float_data, &accuracy, (inv_time_t*)&timestamp)) {
          MPL_LOGI("Linear Accel: %7.5f %7.5f %7.5f\r\n",
              float_data[0], float_data[1], float_data[2]);                                        
         }
    }
    if (hal.report & PRINT_GRAVITY_VECTOR) {
            if (inv_get_sensor_type_gravity(float_data, &accuracy,
                (inv_time_t*)&timestamp))
              MPL_LOGI("Gravity Vector: %7.5f %7.5f %7.5f\r\n",
                  float_data[0], float_data[1], float_data[2]);
    }
    if (hal.report & PRINT_PEDO) {
        unsigned long timestamp;
        get_tick_count(&timestamp);
        if (timestamp > hal.next_pedo_ms) {
            hal.next_pedo_ms = timestamp + PEDO_READ_MS;
            unsigned long step_count, walk_time;
            dmp_get_pedometer_step_count(&step_count);
            dmp_get_pedometer_walk_time(&walk_time);
            MPL_LOGI("Walked %ld steps over %ld milliseconds..\n", step_count,
            walk_time);
        }
    }

    /* Whenever the MPL detects a change in motion state, the application can
     * be notified. For this example, we use an LED to represent the current
     * motion state.
     */
    msg = inv_get_message_level_0(INV_MSG_MOTION_EVENT |
            INV_MSG_NO_MOTION_EVENT);
    if (msg) {
        if (msg & INV_MSG_MOTION_EVENT) {
            MPL_LOGI("Motion!\n");
        } else if (msg & INV_MSG_NO_MOTION_EVENT) {
            MPL_LOGI("No motion!\n");
        }
    }
}

#ifdef COMPASS_ENABLED
void send_status_compass() {
  long data[3] = { 0 };
  int8_t accuracy = { 0 };
  unsigned long timestamp;
  inv_get_compass_set(data, &accuracy, (inv_time_t*) &timestamp);
  MPL_LOGI("Compass: %7.4f %7.4f %7.4f ",
      data[0]/65536.f, data[1]/65536.f, data[2]/65536.f);
  MPL_LOGI("Accuracy= %d\r\n", accuracy);

}
#endif

/* Handle sensor on/off combinations. */
static void setup_gyro(void)
{
    unsigned char mask = 0, lp_accel_was_on = 0;
    if (hal.sensors & ACCEL_ON)
        mask |= INV_XYZ_ACCEL;
    if (hal.sensors & GYRO_ON) {
        mask |= INV_XYZ_GYRO;
        lp_accel_was_on |= hal.lp_accel_mode;
    }
#ifdef COMPASS_ENABLED
    if (hal.sensors & COMPASS_ON) {
        mask |= INV_XYZ_COMPASS;
        lp_accel_was_on |= hal.lp_accel_mode;
    }
#endif
    /* If you need a power transition, this function should be called with a
     * mask of the sensors still enabled. The driver turns off any sensors
     * excluded from this mask.
     */
    mpu_set_sensors(mask);
    mpu_configure_fifo(mask);
    if (lp_accel_was_on) {
        unsigned short rate;
        hal.lp_accel_mode = 0;
        /* Switching out of LP accel, notify MPL of new accel sampling rate. */
        mpu_get_sample_rate(&rate);
        inv_set_accel_sample_rate(1000000L / rate);
    }
}

static void tap_cb(unsigned char direction, unsigned char count)
{
    switch (direction) {
    case TAP_X_UP:
        MPL_LOGI("Tap X+ ");
        break;
    case TAP_X_DOWN:
        MPL_LOGI("Tap X- ");
        break;
    case TAP_Y_UP:
        MPL_LOGI("Tap Y+ ");
        break;
    case TAP_Y_DOWN:
        MPL_LOGI("Tap Y- ");
        break;
    case TAP_Z_UP:
        MPL_LOGI("Tap Z+ ");
        break;
    case TAP_Z_DOWN:
        MPL_LOGI("Tap Z- ");
        break;
    default:
        return;
    }
    MPL_LOGI("x%d\n", count);
    return;
}

static void android_orient_cb(unsigned char orientation)
{
  switch (orientation) {
  case ANDROID_ORIENT_PORTRAIT:
        MPL_LOGI("Portrait\n");
        break;
  case ANDROID_ORIENT_LANDSCAPE:
        MPL_LOGI("Landscape\n");
        break;
  case ANDROID_ORIENT_REVERSE_PORTRAIT:
        MPL_LOGI("Reverse Portrait\n");
        break;
  case ANDROID_ORIENT_REVERSE_LANDSCAPE:
        MPL_LOGI("Reverse Landscape\n");
        break;
  default:
    return;
  }
}


static inline void run_self_test(void)
{
    int result;
    long gyro[3], accel[3];

#if defined (MPU6500) || defined (MPU9250)
    result = mpu_run_6500_self_test(gyro, accel, 0);
#elif defined (MPU6050) || defined (MPU9150)
    result = mpu_run_self_test(gyro, accel);
#endif
    if (result == 0x7) {
    MPL_LOGI("Passed!\n");
        MPL_LOGI("accel: %7.4f %7.4f %7.4f\n",
                    accel[0]/65536.f,
                    accel[1]/65536.f,
                    accel[2]/65536.f);
        MPL_LOGI("gyro: %7.4f %7.4f %7.4f\n",
                    gyro[0]/65536.f,
                    gyro[1]/65536.f,
                    gyro[2]/65536.f);
        /* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/

#ifdef USE_CAL_HW_REGISTERS
        /*
         * This portion of the code uses the HW offset registers that are in the MPUxxxx devices
         * instead of pushing the cal data to the MPL software library
         */
        unsigned char i = 0;

        for(i = 0; i<3; i++) {
          gyro[i] = (long)(gyro[i] * 32.8f); //convert to +-1000dps
          accel[i] *=  2048.f; //convert to +-16G (bug fix from +-8G)
          accel[i] = accel[i] >> 16;
          gyro[i] = (long)(gyro[i] >> 16);
        }

        mpu_set_gyro_bias_reg(gyro);

#if defined (MPU6500) || defined (MPU9250)
        mpu_set_accel_bias_6500_reg(accel);
#elif defined (MPU6050) || defined (MPU9150)
        mpu_set_accel_bias_6050_reg(accel);
#endif
#else
        /* Push the calibrated data to the MPL library.
         *
         * MPL expects biases in hardware units << 16, but self test returns
     * biases in g's << 16.
     */
      unsigned short accel_sens;
      float gyro_sens;

    mpu_get_accel_sens(&accel_sens);
    accel[0] *= accel_sens;
    accel[1] *= accel_sens;
    accel[2] *= accel_sens;
    inv_set_accel_bias(accel, 3);
    mpu_get_gyro_sens(&gyro_sens);
    gyro[0] = (long) (gyro[0] * gyro_sens);
    gyro[1] = (long) (gyro[1] * gyro_sens);
    gyro[2] = (long) (gyro[2] * gyro_sens);
    inv_set_gyro_bias(gyro, 3);
#endif
    }
    else {
            if (!(result & 0x1))
                MPL_LOGE("Gyro failed.\n");
            if (!(result & 0x2))
                MPL_LOGE("Accel failed.\n");
            if (!(result & 0x4))
                MPL_LOGE("Compass failed.\n");
     }

}

// static void handle_input(void)
// {
//     char c;
//     size_t store_size;
//     const unsigned char header[3] = "inv";

//     /* Read incoming byte and check for header.
//      * Technically, the MSP430 USB stack can handle more than one byte at a
//      * time. This example allows for easily switching to UART if porting to a
//      * different microcontroller.
//      */
//     rx_new = 0;
//     cdcReceiveDataInBuffer((BYTE*)&c, 1, CDC0_INTFNUM);
//     if (hal.rx.header[0] == header[0]) {
//         if (hal.rx.header[1] == header[1]) {
//             if (hal.rx.header[2] == header[2]) {
//                 memset(&hal.rx.header, 0, sizeof(hal.rx.header));
//                 hal.rx.cmd = c;
//             } else if (c == header[2])
//                 hal.rx.header[2] = c;
//             else
//                 memset(&hal.rx.header, 0, sizeof(hal.rx.header));
//         } else if (c == header[1])
//             hal.rx.header[1] = c;
//         else
//             memset(&hal.rx.header, 0, sizeof(hal.rx.header));
//     } else if (c == header[0])
//         hal.rx.header[0] = header[0];
//     if (!hal.rx.cmd)
//         return;

//     switch (hal.rx.cmd) {
//     /* These commands turn off individual sensors. */
//     case '8':
//         hal.sensors ^= ACCEL_ON;
//         setup_gyro();
//         if (!(hal.sensors & ACCEL_ON))
//             inv_accel_was_turned_off();
//         break;
//     case '9':
//         hal.sensors ^= GYRO_ON;
//         setup_gyro();
//         if (!(hal.sensors & GYRO_ON))
//             inv_gyro_was_turned_off();
//         break;
// #ifdef COMPASS_ENABLED
//     case '0':
//         hal.sensors ^= COMPASS_ON;
//         setup_gyro();
//         if (!(hal.sensors & COMPASS_ON))
//             inv_compass_was_turned_off();
//         break;
// #endif
//     /* The commands send individual sensor data or fused data to the PC. */
//     case 'a':
//         hal.report ^= PRINT_ACCEL;
//         break;
//     case 'g':
//         hal.report ^= PRINT_GYRO;
//         break;
// #ifdef COMPASS_ENABLED
//     case 'c':
//         hal.report ^= PRINT_COMPASS;
//         break;
// #endif
//     case 'e':
//         hal.report ^= PRINT_EULER;
//         break;
//     case 'r':
//         hal.report ^= PRINT_ROT_MAT;
//         break;
//     case 'q':
//         hal.report ^= PRINT_QUAT;
//         break;
//     case 'h':
//         hal.report ^= PRINT_HEADING;
//         break;
//     case 'i':
//         hal.report ^= PRINT_LINEAR_ACCEL;
//         break;
//     case 'o':
//         hal.report ^= PRINT_GRAVITY_VECTOR;
//         break;
// #ifdef COMPASS_ENABLED
//   case 'w':
//     send_status_compass();
//     break;
// #endif
//     /* This command prints out the value of each gyro register for debugging.
//      * If logging is disabled, this function has no effect.
//      */
//     case 'd':
//         mpu_reg_dump();
//         break;
//     /* Test out low-power accel mode. */
//     case 'p':
//         if (hal.dmp_on)
//             /* LP accel is not compatible with the DMP. */
//             break;
//         mpu_lp_accel_mode(20);
//         /* When LP accel mode is enabled, the driver automatically configures
//          * the hardware for latched interrupts. However, the MCU sometimes
//          * misses the rising/falling edge, and the hal.new_gyro flag is never
//          * set. To avoid getting locked in this state, we're overriding the
//          * driver's configuration and sticking to unlatched interrupt mode.
//          *
//          * TODO: The MCU supports level-triggered interrupts.
//          */
//         mpu_set_int_latched(0);
//         hal.sensors &= ~(GYRO_ON|COMPASS_ON);
//         hal.sensors |= ACCEL_ON;
//         hal.lp_accel_mode = 1;
//         inv_gyro_was_turned_off();
//         inv_compass_was_turned_off();
//         break;
//     /* This snippet of code shows how to load and store calibration data from
//      * the MPL. For the MSP430, the flash segment must be unlocked before
//      * reading/writing and locked when no longer in use. When porting to a
//      * different microcontroller, flash memory might be accessible at anytime,
//      * or may not be available at all.
//      */
//     case 'l':
//         inv_get_mpl_state_size(&store_size);
//         if (store_size > FLASH_SIZE) {
//             MPL_LOGE("Calibration data exceeds available memory.\n");
//             break;
//         }
//         FCTL3 = FWKEY;
//         if(inv_load_mpl_states(FLASH_MEM_START, store_size))
//           MPL_LOGE("Error Loading Cal Data.\n");
//         FCTL3 = FWKEY + LOCK;
//         inv_accel_was_turned_off();
//         inv_gyro_was_turned_off();
//         inv_compass_was_turned_off();
//         break;
//     case 's':
//         inv_get_mpl_state_size(&store_size);
//         if (store_size > FLASH_SIZE) {
//             MPL_LOGE("Calibration data exceeds available memory.\n");
//             return;
//         } else {
//             unsigned char mpl_states[100], tries = 5, erase_result;
//             inv_save_mpl_states(mpl_states, store_size);
//             while (tries--) {
//                 /* Multiple attempts to erase current data. */
//                 Flash_SegmentErase((uint16_t*)FLASH_MEM_START);
//                 erase_result = Flash_EraseCheck((uint16_t*)FLASH_MEM_START,
//                     store_size>>1);
//                 if (erase_result == FLASH_STATUS_OK)
//                     break;
//             }
//             if (erase_result == FLASH_STATUS_ERROR) {
//                 MPL_LOGE("Could not erase user page for calibration "
//                     "storage.\n");
//                 break;
//             }
//             FlashWrite_8(mpl_states, FLASH_MEM_START, store_size);
//         }
//         inv_accel_was_turned_off();
//         inv_gyro_was_turned_off();
//         inv_compass_was_turned_off();
//         break;
//     /* The hardware self test can be run without any interaction with the
//      * MPL since it's completely localized in the gyro driver. Logging is
//      * assumed to be enabled; otherwise, a couple LEDs could probably be used
//      * here to display the test results.
//      */
//     case 't':
//         run_self_test();
//         /* Let MPL know that contiguity was broken. */
//         inv_accel_was_turned_off();
//         inv_gyro_was_turned_off();
//         inv_compass_was_turned_off();
//         break;
//     /* Depending on your application, sensor data may be needed at a faster or
//      * slower rate. These commands can speed up or slow down the rate at which
//      * the sensor data is pushed to the MPL.
//      *
//      * In this example, the compass rate is never changed.
//      */
//     case '1':
//         if (hal.dmp_on) {
//             dmp_set_fifo_rate(10);
//             inv_set_quat_sample_rate(100000L);
//         } else
//             mpu_set_sample_rate(10);
//         inv_set_gyro_sample_rate(100000L);
//         inv_set_accel_sample_rate(100000L);
//         break;
//     case '2':
//         if (hal.dmp_on) {
//             dmp_set_fifo_rate(20);
//             inv_set_quat_sample_rate(50000L);
//         } else
//             mpu_set_sample_rate(20);
//         inv_set_gyro_sample_rate(50000L);
//         inv_set_accel_sample_rate(50000L);
//         break;
//     case '3':
//         if (hal.dmp_on) {
//             dmp_set_fifo_rate(40);
//             inv_set_quat_sample_rate(25000L);
//         } else
//             mpu_set_sample_rate(40);
//         inv_set_gyro_sample_rate(25000L);
//         inv_set_accel_sample_rate(25000L);
//         break;
//     case '4':
//         if (hal.dmp_on) {
//             dmp_set_fifo_rate(50);
//             inv_set_quat_sample_rate(20000L);
//         } else
//             mpu_set_sample_rate(50);
//         inv_set_gyro_sample_rate(20000L);
//         inv_set_accel_sample_rate(20000L);
//         break;
//     case '5':
//         if (hal.dmp_on) {
//             dmp_set_fifo_rate(100);
//             inv_set_quat_sample_rate(10000L);
//         } else
//             mpu_set_sample_rate(100);
//         inv_set_gyro_sample_rate(10000L);
//         inv_set_accel_sample_rate(10000L);
//         break;
//   case ',':
//         /* Set hardware to interrupt on gesture event only. This feature is
//          * useful for keeping the MCU asleep until the DMP detects as a tap or
//          * orientation event.
//          */
//         dmp_set_interrupt_mode(DMP_INT_GESTURE);
//         break;
//     case '.':
//         /* Set hardware to interrupt periodically. */
//         dmp_set_interrupt_mode(DMP_INT_CONTINUOUS);
//         break;
//     case '6':
//         /* Toggle pedometer display. */
//         hal.report ^= PRINT_PEDO;
//         break;
//     case '7':
//         /* Reset pedometer. */
//         dmp_set_pedometer_step_count(0);
//         dmp_set_pedometer_walk_time(0);
//         break;
//     case 'f':
//         if (hal.lp_accel_mode)
//             /* LP accel is not compatible with the DMP. */
//             return;
//         /* Toggle DMP. */
//         if (hal.dmp_on) {
//             unsigned short dmp_rate;
//             unsigned char mask = 0;
//             hal.dmp_on = 0;
//             mpu_set_dmp_state(0);
//             /* Restore FIFO settings. */
//             if (hal.sensors & ACCEL_ON)
//                 mask |= INV_XYZ_ACCEL;
//             if (hal.sensors & GYRO_ON)
//                 mask |= INV_XYZ_GYRO;
//             if (hal.sensors & COMPASS_ON)
//                 mask |= INV_XYZ_COMPASS;
//             mpu_configure_fifo(mask);
//             /* When the DMP is used, the hardware sampling rate is fixed at
//              * 200Hz, and the DMP is configured to downsample the FIFO output
//              * using the function dmp_set_fifo_rate. However, when the DMP is
//              * turned off, the sampling rate remains at 200Hz. This could be
//              * handled in inv_mpu.c, but it would need to know that
//              * inv_mpu_dmp_motion_driver.c exists. To avoid this, we'll just
//              * put the extra logic in the application layer.
//              */
//             dmp_get_fifo_rate(&dmp_rate);
//             mpu_set_sample_rate(dmp_rate);
//             inv_quaternion_sensor_was_turned_off();
//             MPL_LOGI("DMP disabled.\n");
//         } else {
//             unsigned short sample_rate;
//             hal.dmp_on = 1;
//             /* Preserve current FIFO rate. */
//             mpu_get_sample_rate(&sample_rate);
//             dmp_set_fifo_rate(sample_rate);
//             inv_set_quat_sample_rate(1000000L / sample_rate);
//             mpu_set_dmp_state(1);
//             MPL_LOGI("DMP enabled.\n");
//         }
//         break;
//     case 'm':
//         /* Test the motion interrupt hardware feature. */
//     #ifndef MPU6050 // not enabled for 6050 product
//     hal.motion_int_mode = 1;
//     #endif 
//         break;
//     case 'x':
//         msp430_reset();
//         break;
//     case 'v':
//         /* Toggle LP quaternion.
//          * The DMP features can be enabled/disabled at runtime. Use this same
//          * approach for other features.
//          */
//         hal.dmp_features ^= DMP_FEATURE_6X_LP_QUAT;
//         dmp_enable_feature(hal.dmp_features);
//         if (!(hal.dmp_features & DMP_FEATURE_6X_LP_QUAT)) {
//             inv_quaternion_sensor_was_turned_off();
//             MPL_LOGI("LP quaternion disabled.\n");
//         } else
//             MPL_LOGI("LP quaternion enabled.\n");
//         break;
//     default:
//         break;
//     }
//     hal.rx.cmd = 0;
// }

/* Every time new gyro data is available, this function is called in an
 * ISR context. In this example, it sets a flag protecting the FIFO read
 * function.
 */
static void gyro_data_ready_cb(void)
{
    hal.new_gyro = 1;
}

/* Set up MSP430 peripherals. */
// static inline void platform_init(void)
// {
//   WDTCTL = WDTPW | WDTHOLD;
//     SetVCore(2);
//     msp430_clock_init(12000000L, 2);
//     if (USB_init() != kUSB_succeed)
//         msp430_reset();
//     msp430_i2c_enable();
//     msp430_int_init();
//     P1DIR |= 0x1C;
//     P1OUT |= 0x04;
//     P1OUT &= ~0x18;

//     USB_setEnabledEvents(kUSB_allUsbEvents);
//     if (USB_connectionInfo() & kUSB_vbusPresent){
//         if (USB_enable() == kUSB_succeed){
//             USB_reset();
//             USB_connect();
//         } else
//             msp430_reset();
//     }
// }

int get_tick_count(unsigned long *count)
{
    count[0] = HAL_GetTick();
    return 0;
}

void inv_init(void)
{
  inv_error_t result;
  unsigned char accel_fsr,  new_temp = 0;
  unsigned short gyro_rate, gyro_fsr;
  unsigned long timestamp;
  struct int_param_s int_param;

#ifdef COMPASS_ENABLED
  unsigned char new_compass = 0;
  unsigned short compass_fsr;
#endif
  // while(1)
  // {
  //   printf("%d\r\n", HAL_GetTick());
  //   HAL_Delay(1000);
  // }
  result = mpu_init(NULL);
  if (result) {
      printf("Could not initialize gyro.\r\n");
      return;
  }
  // else
  // {
  //   printf("mpu init success\r\n");

  //   inv_enable_9x_sensor_fusion();
  //   inv_enable_fast_nomot();
  //   inv_enable_gyro_tc();
  //   inv_enable_vector_compass_cal();
  //   inv_enable_magnetic_disturbance();
  //   inv_enable_eMPL_outputs();
  // }

  

  result = inv_init_mpl();
    if (result) {
        printf("Could not initialize MPL.\n");
        
    }

    /* Compute 6-axis and 9-axis quaternions. */
    inv_enable_quaternion();
    inv_enable_9x_sensor_fusion();
    /* The MPL expects compass data at a constant rate (matching the rate
     * passed to inv_set_compass_sample_rate). If this is an issue for your
     * application, call this function, and the MPL will depend on the
     * timestamps passed to inv_build_compass instead.
     *
     * inv_9x_fusion_use_timestamps(1);
     */

    /* This function has been deprecated.
     * inv_enable_no_gyro_fusion();
     */

    /* Update gyro biases when not in motion.
     * WARNING: These algorithms are mutually exclusive.
     */
    inv_enable_fast_nomot();
    /* inv_enable_motion_no_motion(); */
    /* inv_set_no_motion_time(1000); */

    /* Update gyro biases when temperature changes. */
    inv_enable_gyro_tc();

    /* This algorithm updates the accel biases when in motion. A more accurate
     * bias measurement can be made when running the self-test (see case 't' in
     * handle_input), but this algorithm can be enabled if the self-test can't
     * be executed in your application.
     *
     * inv_enable_in_use_auto_calibration();
     */
#ifdef COMPASS_ENABLED
    /* Compass calibration algorithms. */
    inv_enable_vector_compass_cal();
    inv_enable_magnetic_disturbance();
#endif
    /* If you need to estimate your heading before the compass is calibrated,
     * enable this algorithm. It becomes useless after a good figure-eight is
     * detected, so we'll just leave it out to save memory.
     * inv_enable_heading_from_gyro();
     */

    /* Allows use of the MPL APIs in read_from_mpl. */
    inv_enable_eMPL_outputs();

    result = inv_start_mpl();
    if (result == INV_ERROR_NOT_AUTHORIZED) {
        while (1) {
            printf("Not authorized.\n");
            inv_mpl_delay_ms(5000);
        }
    }
    if (result) {
        printf("Could not start the MPL.\n");
        
    }

    /* Get/set hardware configuration. Start gyro. */
    /* Wake up all sensors. */
#ifdef COMPASS_ENABLED
    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
#else
    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
#endif
    /* Push both gyro and accel data into the FIFO. */
    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    mpu_set_sample_rate(DEFAULT_MPU_HZ);
#ifdef COMPASS_ENABLED
    /* The compass sampling rate can be less than the gyro/accel sampling rate.
     * Use this function for proper power management.
     */
    mpu_set_compass_sample_rate(1000 / COMPASS_READ_MS);
#endif
    /* Read back configuration in case it was set improperly. */
    mpu_get_sample_rate(&gyro_rate);
    mpu_get_gyro_fsr(&gyro_fsr);
    mpu_get_accel_fsr(&accel_fsr);
#ifdef COMPASS_ENABLED
    mpu_get_compass_fsr(&compass_fsr);
#endif
    /* Sync driver configuration with MPL. */
    /* Sample rate expected in microseconds. */
    inv_set_gyro_sample_rate(1000000L / gyro_rate);
    inv_set_accel_sample_rate(1000000L / gyro_rate);
#ifdef COMPASS_ENABLED
    /* The compass rate is independent of the gyro and accel rates. As long as
     * inv_set_compass_sample_rate is called with the correct value, the 9-axis
     * fusion algorithm's compass correction gain will work properly.
     */
    inv_set_compass_sample_rate(COMPASS_READ_MS * 1000L);
#endif
    /* Set chip-to-body orientation matrix.
     * Set hardware units to dps/g's/degrees scaling factor.
     */
    inv_set_gyro_orientation_and_scale(
            inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
            (long)gyro_fsr<<15);
    inv_set_accel_orientation_and_scale(
            inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
            (long)accel_fsr<<15);
#ifdef COMPASS_ENABLED
    inv_set_compass_orientation_and_scale(
            inv_orientation_matrix_to_scalar(compass_pdata.orientation),
            (long)compass_fsr<<15);
#endif
    /* Initialize HAL state variables. */
#ifdef COMPASS_ENABLED
    hal.sensors = ACCEL_ON | GYRO_ON | COMPASS_ON;
#else
    hal.sensors = ACCEL_ON | GYRO_ON;
#endif
    hal.dmp_on = 0;
    hal.report = 0;
    hal.rx.cmd = 0;
    hal.next_pedo_ms = 0;
    hal.next_compass_ms = 0;
    hal.next_temp_ms = 0;

    /* Compass reads are handled by scheduler. */
    get_tick_count(&timestamp);

    /* To initialize the DMP:
     * 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in
     *    inv_mpu_dmp_motion_driver.h into the MPU memory.
     * 2. Push the gyro and accel orientation matrix to the DMP.
     * 3. Register gesture callbacks. Don't worry, these callbacks won't be
     *    executed unless the corresponding feature is enabled.
     * 4. Call dmp_enable_feature(mask) to enable different features.
     * 5. Call dmp_set_fifo_rate(freq) to select a DMP output rate.
     * 6. Call any feature-specific control functions.
     *
     * To enable the DMP, just call mpu_set_dmp_state(1). This function can
     * be called repeatedly to enable and disable the DMP at runtime.
     *
     * The following is a short summary of the features supported in the DMP
     * image provided in inv_mpu_dmp_motion_driver.c:
     * DMP_FEATURE_LP_QUAT: Generate a gyro-only quaternion on the DMP at
     * 200Hz. Integrating the gyro data at higher rates reduces numerical
     * errors (compared to integration on the MCU at a lower sampling rate).
     * DMP_FEATURE_6X_LP_QUAT: Generate a gyro/accel quaternion on the DMP at
     * 200Hz. Cannot be used in combination with DMP_FEATURE_LP_QUAT.
     * DMP_FEATURE_TAP: Detect taps along the X, Y, and Z axes.
     * DMP_FEATURE_ANDROID_ORIENT: Google's screen rotation algorithm. Triggers
     * an event at the four orientations where the screen should rotate.
     * DMP_FEATURE_GYRO_CAL: Calibrates the gyro data after eight seconds of
     * no motion.
     * DMP_FEATURE_SEND_RAW_ACCEL: Add raw accelerometer data to the FIFO.
     * DMP_FEATURE_SEND_RAW_GYRO: Add raw gyro data to the FIFO.
     * DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot
     * be used in combination with DMP_FEATURE_SEND_RAW_GYRO.
     */
    if (dmp_load_motion_driver_firmware()) {
            printf("Could not download DMP.\n");
            
    }
    dmp_set_orientation(
        inv_orientation_matrix_to_scalar(gyro_pdata.orientation));
    dmp_register_tap_cb(tap_cb);
    dmp_register_android_orient_cb(android_orient_cb);
    /*
     * Known Bug -
     * DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate
     * specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once
     * a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz
     * there will be a 25Hz interrupt from the MPU device.
     *
     * There is a known issue in which if you do not enable DMP_FEATURE_TAP
     * then the interrupts will be at 200Hz even if fifo rate
     * is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP
     *
     * DMP sensor fusion works only with gyro at +-2000dps and accel +-2G
     */
    hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
        DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
        DMP_FEATURE_GYRO_CAL;
    dmp_enable_feature(hal.dmp_features);
    dmp_set_fifo_rate(DEFAULT_MPU_HZ);
    inv_set_quat_sample_rate(1000000L / DEFAULT_MPU_HZ);
    mpu_set_dmp_state(1);
    hal.dmp_on = 1;

    printf("MPL init succss\r\n");
    // __enable_interrupt();

    /* Wait for enumeration. */
    // while (USB_connectionState() != ST_ENUM_ACTIVE);

//     while (1) {
//         unsigned long sensor_timestamp;
//         int new_data = 0;
//         // if (rx_new)
//         //      A byte has been received via USB. See handle_input for a list of
//         //      * valid commands.
             
//         //     handle_input();
//         get_tick_count(&timestamp);

// #ifdef COMPASS_ENABLED
//         /* We're not using a data ready interrupt for the compass, so we'll
//          * make our compass reads timer-based instead.
//          */
//         if ((timestamp > hal.next_compass_ms) && !hal.lp_accel_mode &&
//             hal.new_gyro && (hal.sensors & COMPASS_ON)) {
//             hal.next_compass_ms = timestamp + COMPASS_READ_MS;
//             new_compass = 1;
//         }
// #endif
//         /* Temperature data doesn't need to be read with every gyro sample.
//          * Let's make them timer-based like the compass reads.
//          */
//         if (timestamp > hal.next_temp_ms) {
//             hal.next_temp_ms = timestamp + TEMP_READ_MS;
//             new_temp = 1;
//         }

//         if (hal.motion_int_mode) {
//             /* Enable motion interrupt. */
//             mpu_lp_motion_interrupt(500, 1, 5);
//             /* Notify the MPL that contiguity was broken. */
//             inv_accel_was_turned_off();
//             inv_gyro_was_turned_off();
//             inv_compass_was_turned_off();
//             inv_quaternion_sensor_was_turned_off();
//             /* Wait for the MPU interrupt. */
//             while (!hal.new_gyro)
//             {
//             	printf("%s, %d\r\n", __FILE__, __LINE__);
//                 // __bis_SR_register(LPM0_bits + GIE);
//             }
//             /* Restore the previous sensor configuration. */
//             mpu_lp_motion_interrupt(0, 0, 0);
//             hal.motion_int_mode = 0;
//         }

//         // if (!hal.sensors || !hal.new_gyro) {
//         //      Put the MSP430 to sleep until a timer interrupt or data ready
//         //      * interrupt is detected.
             
//         //     // __bis_SR_register(LPM0_bits + GIE);
//         //     printf("%s, %d\r\n", __FILE__, __LINE__);
//         //     continue;
//         // }

//         if (hal.new_gyro && hal.lp_accel_mode) {
//             short accel_short[3];
//             long accel[3];
//             mpu_get_accel_reg(accel_short, &sensor_timestamp);
//             accel[0] = (long)accel_short[0];
//             accel[1] = (long)accel_short[1];
//             accel[2] = (long)accel_short[2];
//             inv_build_accel(accel, 0, sensor_timestamp);
//             new_data = 1;
//             hal.new_gyro = 0;
//         } else if (hal.new_gyro && hal.dmp_on) {
//             short gyro[3], accel_short[3], sensors;
//             unsigned char more;
//             long accel[3], quat[4], temperature;
//             /* This function gets new data from the FIFO when the DMP is in
//              * use. The FIFO can contain any combination of gyro, accel,
//              * quaternion, and gesture data. The sensors parameter tells the
//              * caller which data fields were actually populated with new data.
//              * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
//              * the FIFO isn't being filled with accel data.
//              * The driver parses the gesture data to determine if a gesture
//              * event has occurred; on an event, the application will be notified
//              * via a callback (assuming that a callback function was properly
//              * registered). The more parameter is non-zero if there are
//              * leftover packets in the FIFO.
//              */
//             dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);
//             if (!more)
//                 hal.new_gyro = 0;
//             if (sensors & INV_XYZ_GYRO) {
//                 /* Push the new data to the MPL. */
//                 inv_build_gyro(gyro, sensor_timestamp);
//                 new_data = 1;
//                 if (new_temp) {
//                     new_temp = 0;
//                      // Temperature only used for gyro temp comp. 
//                     mpu_get_temperature(&temperature, &sensor_timestamp);
//                     inv_build_temp(temperature, sensor_timestamp);

//                     printf("temp: %d\r\n", temperature);
//                 }
//             }
//             if (sensors & INV_XYZ_ACCEL) {
//                 accel[0] = (long)accel_short[0];
//                 accel[1] = (long)accel_short[1];
//                 accel[2] = (long)accel_short[2];
//                 inv_build_accel(accel, 0, sensor_timestamp);
//                 new_data = 1;
//             }
//             if (sensors & INV_WXYZ_QUAT) {
//                 inv_build_quat(quat, 0, sensor_timestamp);
//                 new_data = 1;
//             }
//         } else if (hal.new_gyro) {
//             short gyro[3], accel_short[3];
//             unsigned char sensors, more;
//             long accel[3], temperature;
//             /* This function gets new data from the FIFO. The FIFO can contain
//              * gyro, accel, both, or neither. The sensors parameter tells the
//              * caller which data fields were actually populated with new data.
//              * For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
//              * being filled with accel data. The more parameter is non-zero if
//              * there are leftover packets in the FIFO. The HAL can use this
//              * information to increase the frequency at which this function is
//              * called.
//              */
//             hal.new_gyro = 0;
//             mpu_read_fifo(gyro, accel_short, &sensor_timestamp,
//                 &sensors, &more);
//             if (more)
//                 hal.new_gyro = 1;
//             if (sensors & INV_XYZ_GYRO) {
//                 /* Push the new data to the MPL. */
//                 inv_build_gyro(gyro, sensor_timestamp);
//                 new_data = 1;
//                 if (new_temp) {
//                     new_temp = 0;
//                     /* Temperature only used for gyro temp comp. */
//                     mpu_get_temperature(&temperature, &sensor_timestamp);
//                     inv_build_temp(temperature, sensor_timestamp);
//                 }
//             }
//             if (sensors & INV_XYZ_ACCEL) {
//                 accel[0] = (long)accel_short[0];
//                 accel[1] = (long)accel_short[1];
//                 accel[2] = (long)accel_short[2];
//                 inv_build_accel(accel, 0, sensor_timestamp);
//                 new_data = 1;
//             }
//         }
// #ifdef COMPASS_ENABLED
//         if (new_compass) {
//             short compass_short[3];
//             long compass[3];
//             new_compass = 0;
//             /* For any MPU device with an AKM on the auxiliary I2C bus, the raw
//              * magnetometer registers are copied to special gyro registers.
//              */
//             if (!mpu_get_compass_reg(compass_short, &sensor_timestamp)) {
//                 compass[0] = (long)compass_short[0];
//                 compass[1] = (long)compass_short[1];
//                 compass[2] = (long)compass_short[2];
//                 /* NOTE: If using a third-party compass calibration library,
//                  * pass in the compass data in uT * 2^16 and set the second
//                  * parameter to INV_CALIBRATED | acc, where acc is the
//                  * accuracy from 0 to 3.
//                  */
//                 inv_build_compass(compass, 0, sensor_timestamp);
//             }
//             new_data = 1;
//         }
// #endif
//         if (new_data) {
//             if(inv_execute_on_data()) {
//                 printf("ERROR execute on data\n");
//             }

//             /* This function reads bias-compensated sensor data and sensor
//              * fusion outputs from the MPL. The outputs are formatted as seen
//              * in eMPL_outputs.c. This function only needs to be called at the
//              * rate requested by the host.
//              */
//             read_from_mpl();
//         }
//     }



}

signed char mpu_mpl_get_data(float *pitch,float *roll,float *yaw, int *acc)
{
    unsigned long sensor_timestamp,timestamp;
    short gyro[3], accel_short[3],compass_short[3],sensors;
    unsigned char more;
    long compass[3],accel[3],quat[4],temperature; 
    long data[9];
    int8_t accuracy;
    
    if(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more))return 1;   

    if(sensors&INV_XYZ_GYRO)
    {
        inv_build_gyro(gyro,sensor_timestamp);          //°ÑÐÂÊý¾Ý·¢ËÍ¸øMPL
        mpu_get_temperature(&temperature,&sensor_timestamp);
        inv_build_temp(temperature,sensor_timestamp);   //°ÑÎÂ¶ÈÖµ·¢¸øMPL£¬Ö»ÓÐÍÓÂÝÒÇÐèÒªÎÂ¶ÈÖµ
    }
    
    if(sensors&INV_XYZ_ACCEL)
    {
        accel[0] = (long)accel_short[0];
        accel[1] = (long)accel_short[1];
        accel[2] = (long)accel_short[2];
        inv_build_accel(accel,0,sensor_timestamp);      //°Ñ¼ÓËÙ¶ÈÖµ·¢¸øMPL
    }
    
    if (!mpu_get_compass_reg(compass_short, &sensor_timestamp)) 
    {
        compass[0]=(long)compass_short[0];
        compass[1]=(long)compass_short[1];
        compass[2]=(long)compass_short[2];
        inv_build_compass(compass,0,sensor_timestamp); //°Ñ´ÅÁ¦¼ÆÖµ·¢¸øMPL
    }
    inv_execute_on_data();
    inv_get_sensor_type_euler(data,&accuracy,&timestamp);
    
    *roll  = (data[0]/q16);
    *pitch = -(data[1]/q16);
    *yaw   = -data[2] / q16;
    *acc = (int)accuracy;
    return 0;
}
#endif